// ESC and Motor Test Code for ArduPilot Mega v0.5b
// Kashif Shah
// USI IEEE

#include <Servo.h>

// Establish architechtural flags
#define ATTACH_SERVO 1
#define MOTOR_SPIN 1
#define CALIBRATE_ESC 1

// Establish ESC specific timing parameters
#define esc_min 1100                   // in us, recommended minimum when uncertain
#define esc_neutral 1500               // in us, recommended neutral when uncertain
#define esc_max 1900                   // in us, recommended maximum when uncertain
#define test_timing 1300

// Declare variables
int loop_counter = 0;

// Declare pinouts for ESC
// We are not yet certain of the proper pinouts for servo control,
// however the following pins have made three of the four motors spin

int pin_lift_f = 12; //(maybe starting from 54 instead) // was 44
int pin_lift_r = 13;      // was 45
int pin_roll_l = 45;
int pin_roll_r = 44;

// pin 47 = relay

// Declare ESC's as servos from the arduino servo library
Servo Lift_F;
Servo Lift_R;
Servo Roll_L;
Servo Roll_R;

void setup() {
  // assign pinouts to the ESCs
  if(ATTACH_SERVO == 1) {
     Lift_F.attach(pin_lift_f);
     Lift_R.attach(pin_lift_r);
     Roll_L.attach(pin_roll_l);
     Roll_R.attach(pin_roll_r);
  }
  // begin serial debugging
  Serial.begin(57600);
  delay(2000);
  
  //Calibrate ESCs
  if(CALIBRATE_ESC == 1) {
    Lift_F.writeMicroseconds(esc_max);
    Lift_R.writeMicroseconds(esc_max);
    Roll_L.writeMicroseconds(esc_max);
    Roll_R.writeMicroseconds(esc_max);
    delay(2000);
    Lift_F.writeMicroseconds(esc_min);
    Lift_R.writeMicroseconds(esc_min);
    Roll_L.writeMicroseconds(esc_min);
    Roll_R.writeMicroseconds(esc_min);
    delay(2000);
  }
   
  // Arm ESCs
   Lift_F.writeMicroseconds(esc_min);
   Lift_R.writeMicroseconds(esc_min);
   Roll_L.writeMicroseconds(esc_min);
   Roll_R.writeMicroseconds(esc_min);
   delay(2000);
   
 }

void loop() {
  
  delay(5000); // five second delay
  
  if(loop_counter > 100) {
    Lift_F.writeMicroseconds(esc_min); 
    Lift_R.writeMicroseconds(esc_min); 
    Roll_L.writeMicroseconds(esc_min); 
    Roll_R.writeMicroseconds(esc_min); 
    delay(5000);
    loop_counter = 0;
  }
  
  // send servo control timings to the ESCs
  // assuming that 1500us is neutral, but know range is somewhere between ~1000us to ~2000us
  if(MOTOR_SPIN == 1) {
    Lift_F.writeMicroseconds(test_timing); 
    Lift_R.writeMicroseconds(test_timing); 
    Roll_L.writeMicroseconds(test_timing); 
    Roll_R.writeMicroseconds(test_timing); 
  }
    
  loop_counter++;
  delay(20);
 
  // send serial debugging information
 /* 
  Serial.print("Pan: ");
  Serial.println(pan.read());
  Serial.print("Tilt: ");
  Serial.println(tilt.read());
  delay(1000);
  */
}
